#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/utils.h>
#include <tf2_ros/transform_broadcaster.h>

class TransformBroadcaster : public rclcpp::Node {
public:
  TransformBroadcaster(std::string node_name) : rclcpp::Node(node_name) {
    RCLCPP_INFO(this->get_logger(), "node: %s", node_name.c_str());
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
  }
  void set_transform(geometry_msgs::msg::TransformStamped &transform) {
    // 2, 设置起始位姿数据
    transform.header.frame_id = "world";
    transform.child_frame_id = "robot";
    transform.transform.translation.x = 1.0;
    transform.transform.translation.y = 2.0;
    transform.transform.translation.z = 3.0;
    transform.transform.rotation.x = 0.0;
    transform.transform.rotation.y = 0.0;
    transform.transform.rotation.z = 0.0;
    transform.transform.rotation.w = 1.0;
  }
  void send_transform(geometry_msgs::msg::TransformStamped transform) {
    // 3.1,单次发布----
    tf_broadcaster_->sendTransform(transform);
    RCLCPP_INFO(this->get_logger(), "sent transform");
    // 3.2,连续发布
    while (rclcpp::ok()) {
      tf_broadcaster_->sendTransform(transform);
      RCLCPP_INFO(this->get_logger(), "sent transform");
      rclcpp::sleep_for(std::chrono::seconds(2));
    }
  }

private:
  // 1,;加里广播发布器
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TransformBroadcaster>("transform_broadcaster");
  geometry_msgs::msg::TransformStamped transform;
  node->set_transform(transform);
  node->send_transform(transform);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}